#include "testkinect.h"

#include <glut.h>
#include "KinectWinMgr.h"
#include "settings.h"


TestKinect::TestKinect(int _camWidth, int _camHeight)
	: KinectARGameLogic(_camWidth, _camHeight)
{
	Init();
}

TestKinect::~TestKinect()
{

}

void TestKinect::Init()
{
	KinectARGameLogic::Init();




	glGenTextures(1, &mTexId);
	glBindTexture(GL_TEXTURE_2D, mTexId);
	glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, mCamWidth, mCamHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, mTexData);
}


void TestKinect::Update(float _elapseTime)
{
	KinectARGameLogic::Update(_elapseTime);

	//if (mKinectMgr->IsSkeletonFound())
	{
		//printf("skeleton found!\n");

	}


	if (mTrackingSystem->IsMarkerFound())
	{

		for (int markerInd = 0; markerInd < mTrackingSystem->GetNumDetected(); ++markerInd)
		{
			ARToolKitPlus::ARMarkerInfo mi = mTrackingSystem->GetDetectedMarker(markerInd);
			if (EAGLE_ID == mTrackingSystem->GetVisibleMarkersId().at(markerInd))
			{
				float posx = mTrackingSystem->GetDetectedMarker(markerInd).pos[0];
				float posy = mTrackingSystem->GetDetectedMarker(markerInd).pos[1];

				int scale = 5;
				for (int yind = -scale; yind < scale; ++yind)
				{
					int posCurY =  int(posy + yind);
					if ( posCurY >= 0 && posCurY < mCamHeight)
					{
						for (int xind = -scale; xind < scale; ++xind)
						{
							int posCurX = int(posx + xind);

							if (posCurX >= 0 && posCurX < mCamWidth)
							{
								//printf ("curx = %d, cury = %d\n", posCurX, posCurY);
								mTexData[3 * (posCurY * mCamWidth + posCurX) + 0] = 127;
								mTexData[3 * (posCurY * mCamWidth + posCurX) + 1] = 100;
								mTexData[3 * (posCurY * mCamWidth + posCurX) + 2] = 0;
							}
						}
					}

				}

			}

			if (BIRD_ID == mTrackingSystem->GetVisibleMarkersId().at(markerInd))
			{

				float posx = mTrackingSystem->GetDetectedMarker(markerInd).pos[0];
				float posy = mTrackingSystem->GetDetectedMarker(markerInd).pos[1];

				int scale = 5;
				for (int yind = -scale; yind < scale; ++yind)
				{
					int posCurY =  int(posy + yind);
					if ( posCurY >= 0 && posCurY < mCamHeight)
					{
						for (int xind = -scale; xind < scale; ++xind)
						{
							int posCurX = int(posx + xind);

							if (posCurX >= 0 && posCurX < mCamWidth)
							{
								printf ("curx = %d, cury = %d\n", posCurX, posCurY);
								mTexData[3 * (posCurY * mCamWidth + posCurX) + 0] = 0;
								mTexData[3 * (posCurY * mCamWidth + posCurX) + 1] = 100;
								mTexData[3 * (posCurY * mCamWidth + posCurX) + 2] = 0;
							}
						}
					}

				}

			}
		}
	}



}


void TestKinect::Render()
{
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(512., 384., 927.058, 512., 384., 0., 0., 1., 0.);


	glBindTexture(GL_TEXTURE_2D, mTexId);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, mCamWidth, mCamHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, mTexData); 

	glEnable(GL_TEXTURE_2D);
	glPushMatrix();

	//glLoadIdentity();										// Reset The Modelview Matrix
	glBegin(GL_QUADS);										// Begin Drawing The Background (One Quad)


	glTexCoord2f(0.0f, 0.0f); glVertex3f( mCamWidth, mCamHeight, 0.1f);
	glTexCoord2f(1.0f, 0.0f); glVertex3f( 0.0f,  mCamHeight, 0.1f);
	glTexCoord2f(1.0f, 1.0f); glVertex3f( 0.0f, 0.0f, 0.1f);
	glTexCoord2f(0.0f, 1.0f); glVertex3f( mCamWidth, 0.0f, 0.1f);

	glEnd();												// Done Drawing The Background

	glPopMatrix();
}